package com.googlecode.grs.sensor;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.util.ArrayList;

import com.googlecode.grs.core.MountedSimulatedSensor;
import com.googlecode.grs.environment.PhysicalObject;
import com.googlecode.grs.environment.SimulationEnvironment;
import com.googlecode.grs.event.RangeFinderEvent;
import com.googlecode.grs.event.RangeFinderListener;

/**
 * A RangeFinder measures the distance of line to an intersecting object in its
 * environment It has a relative position to a robot because it is a robot
 * component It draws itself
 * 
 * @author ajc
 * 
 */
public class RangeFinder extends MountedSimulatedSensor {

	public static final int MAXIMUM_RANGE = 400;

	public static final int DISTANCE = 0;

	private Color color;

	private ArrayList<RangeFinderListener> listeners = new ArrayList<RangeFinderListener>();;

	/**
	 * 
	 * @param env
	 *            The environment that the RangeFinder, and its robot, exists in
	 * @param platform
	 *            the servo atop which this sensor is mounted
	 * @param x
	 *            The X component of the RangeFinder's relative position to the
	 *            robot's x component
	 * @param y
	 *            The Y component of the RangeFinder's relative position to the
	 *            robot's y component
	 * @param mountAngle
	 *            The angle relative to the robot's front heading (a
	 *            difference), where 0 is exactly robot heading ,-45 is 45deg.
	 *            left of robot heading)
	 */
	public RangeFinder(SimulationEnvironment env, Servo platform, double x,
			double y, double mountAngle) {
		super(env, platform, x, y, mountAngle);
		color = new Color((float) Math.random(), (float) Math.random(),
				(float) Math.random());
		setState(DISTANCE, MAXIMUM_RANGE);
	}

	private double getDistance() {
		double range = MAXIMUM_RANGE;
		ArrayList<PhysicalObject> known = getPhysicalObjects(range);

		for (PhysicalObject o : known) {
			for (int i = 0; i < range; i++) {
				if (o.intersects(robotX()
						+ ((x + i) * (Math.cos(getHeading()))), robotY()
						+ ((y + i) * (Math.sin(getHeading()))))) {
					range = Math.min(i, range);
					break;
				}
			}

		}
		return range;
	}

	@Override
	public void poll() {
		setState(DISTANCE, getDistance());
	}

	/**
	 * Called internally. Notifies listeners
	 */
	@Override
	protected void sensorStateChanged(int id, double prevData, double data) {

		if (id == DISTANCE) {

			RangeFinderEvent evt = new RangeFinderEvent(this, prevData, data,
					getRobotOffset(), getHeading());
			for (RangeFinderListener listener : listeners) {
				listener.distanceChanged(evt);
			}
		}

	}

	/**
	 * Adds a provided listener to be notified on RangeFinderEvent.
	 * 
	 * @param listener
	 */
	public void addRangeFinderListener(RangeFinderListener listener) {
		listeners.add(listener);
	}

	/**
	 * Stops sending events to a provided RangeFinderListener
	 * 
	 * @param listener
	 */
	public void removeRangeFinderListener(RangeFinderListener listener) {
		listeners.remove(listener);
	}

	/**
	 * draws the RangeFinder in the environment
	 */
	@Override
	public void draw(Graphics g) {
		g.setColor(Color.BLACK);

		Graphics2D g2d = (Graphics2D) g;
		int width = 3;
		g2d.setStroke(new BasicStroke(width));
		g.drawLine(
				(int) (robotX() + ((x + 5) * Math.cos(getHeading()))),
				(int) (robotY() + ((y + 5) * Math.sin(getHeading()))),
				(int) (robotX() + ((x + Math.min(10, getState(DISTANCE))) * Math
						.cos(getHeading()))), (int) (robotY() + ((y + Math.min(
						10, getState(DISTANCE))) * Math.sin(getHeading()))));

		width = 1;
		g2d.setStroke(new BasicStroke(width));
		g.setColor(this.color);
		g.drawLine(
				(int) (robotX() + ((x + Math.min(10, getState(DISTANCE))) * Math
						.cos(getHeading()))), (int) (robotY() + ((y + Math.min(
						10, getState(DISTANCE))) * Math.sin(getHeading()))),
				(int) (robotX() + ((x + getState(DISTANCE)) * Math
						.cos(getHeading()))),
				(int) (robotY() + ((y + getState(DISTANCE)) * Math
						.sin(getHeading()))));

	}

}
